# 测量曲率

import numpy as np
import matplotlib.pyplot as plt

# 生成一些数据用来表示数据线像素

# 用于覆盖与图像相同的 y 范围 生成了一个样本的初始点（0-719）共计720个数据
ploty = np.linspace(0, 719, num=720)
print(ploty)

# 任意的二次系数 表示车道线曲率
quadratic_coeff = 3e-4 


# 对于每一个y位置，在正负 50像素 内生成随机的 x 位置
# 设置线的大概位置 (左侧 x=200 右侧 x=900)
leftx = np.array([200 + (y**2) * quadratic_coeff + np.random.randint(-50, high=51) 
                              for y in ploty])
rightx = np.array([900 + (y**2) * quadratic_coeff + np.random.randint(-50, high=51) 
                                for y in ploty])
leftx = leftx[::-1]  # 颠倒匹配 y
rightx = rightx[::-1]  # 颠倒匹配 y


# 将二阶多项式拟合到每个假车道中的像素位置
left_fit = np.polyfit(ploty, leftx, 2)
print(left_fit)
left_fitx = left_fit[0] * ploty ** 2 + left_fit[1] * ploty + left_fit[2]
right_fit = np.polyfit(ploty, rightx, 2)
right_fitx = right_fit[0] * ploty ** 2 + right_fit[1] * ploty + right_fit[2]

# 可视化
mark_size = 3
plt.plot(leftx, ploty, 'o', color='red', markersize=mark_size)
plt.plot(rightx, ploty, 'o', color='blue', markersize=mark_size)
plt.xlim(0, 1280)
plt.ylim(0, 720)
plt.plot(left_fitx, ploty, color='green', linewidth=3)
plt.plot(right_fitx, ploty, color='green', linewidth=3)
plt.gca().invert_yaxis()
plt.show()

